using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using ElteIk.RoboSoccer.Core.Physics;

namespace ElteIk.RoboSoccer.Core.PhysicsEngine
{
	using PhysicalEntitiesToBodiesMap = Dictionary<IPhysicalObject, Physics2DDotNet.Body>;
    using BodiesToPhysicalEntitiesMap = Dictionary<Physics2DDotNet.Body, IPhysicalObject>;
	using HeadingVectorsMap = Dictionary<IPhysicalObject, Vector2D>;

	/// <summary>
	/// A Physics engine adapter class.
    /// The adapted engine is Physics2DDotNet which can be found at http://groups.google.com/group/physics2ddotnet.
    /// 
	/// It links the Core with the Physics2DDotNet physical library implementing the IPhyicsEngine interface
    /// with lookahead support. (which is not implemented yet due to performance considerations)
    /// 
    /// Terminology: "Core" is the Game and "Library" is the Physics2DDotNet.
    /// 
	/// </summary>
	public class Physics2DDotNetAdapter : ILookAheadPhysicsEngine
    {
        #region Private fields

        private Physics2DDotNet.PhysicsEngine engine;

		private List<IPhysicalObject> objects;
        private PhysicalEntitiesToBodiesMap entitiesMap;
        internal BodiesToPhysicalEntitiesMap bodiesMap;
        private HeadingVectorsMap headingVectors;
        private List<CollisionInfo> collisions;

        private float time = 0.0f;

        #endregion

        #region Engine parameters

        /// <summary>
        /// It is the rate how much the robots will be forced to moved rather towards the heading-direction 
        /// than velocity-direction to decrease drifting.
        /// </summary>
        private float driftingCorrectionFactor = 8f;

        /// <summary>
        /// It defines how much the rotation of the objects will be limited.
        /// </summary>
        private float rotationCorrectionFactor = 1f;

        /// <summary>
        /// It defines the degree of the friction - the bigger this value is the sooner it slows down.
        /// </summary>
        private float frictionCoefficient = 1f;

        private float ballFrictionCoefficient = 0.1f;

        #endregion

        #region IPhysicsEngine Members

        #region Adders

        /// <summary>
		/// Adds a physical object to the model.
		/// </summary>
		/// <param name="physicalObject"></param>
		public void Add(IPhysicalObject physicalObject)
		{
			Physics2DDotNet.Body adaptedBody = CreateBody(physicalObject);
			Add(physicalObject, adaptedBody);
		}

		/// <summary>
		/// Links the core object with the library one.
		/// </summary>
		/// <param name="physicalObject"></param>
		/// <param name="body"></param>
		private void Add(IPhysicalObject physicalObject, Physics2DDotNet.Body body)
		{
			objects.Add(physicalObject);
			entitiesMap.Add(physicalObject, body);
            bodiesMap.Add(body, physicalObject);
			headingVectors.Add(physicalObject, physicalObject.Heading.Normalized);
            body.Collided += new EventHandler<Physics2DDotNet.CollisionEventArgs>(body_Collided);
			engine.AddBody(body);
		}

        private void body_Collided(object sender, Physics2DDotNet.CollisionEventArgs e)
        {
            collisions.Add(
                new CollisionInfo(
                    bodiesMap[e.Contacts[0].Body1], 
                    bodiesMap[e.Contacts[0].Body2],  
                    ConvertVectorLibraryToCore(e.Contacts[0].Position),
                    time));
        }

		/// <summary>
		/// Adds a polygonal object to the model. (like walls)
		/// </summary>
		/// <param name="polygonalObject"></param>
		public void AddPolygon(Polygon polygonalObject)
		{
			Physics2DDotNet.Body adaptedBody = CreateBody(polygonalObject);
			Add(polygonalObject, adaptedBody);
		}

        /// <summary>
        /// Adds an object with circle shape to the model. (ball)
        /// </summary>
        /// <param name="circle"></param>
		public void AddCircle(Circle circle)
		{
			Physics2DDotNet.Body body = CreateBody(circle);
			body.Shape = new Physics2DDotNet.Circle(circle.Radius, 32);
			Add(circle, body);
		}

        /// <summary>
        /// Adds a whole world (having an array of objects) to the model.
        /// </summary>
        /// <param name="world"></param>
		public void AddWorld(World world)
		{
			foreach (IPhysicalObject obj in world.Objects)
			{
				if (obj is Circle)
					AddCircle(obj as Circle);
				else if (obj is Polygon)
					AddPolygon(obj as Polygon);
				else
					Add(obj);
			}
        }

        #endregion

        #region Corrections


        /// <summary>
        /// Physics2DDotNet isn't correctly modelling polygons. Sometimes they get stuck if the polygons consist of too few points.
        /// The correction is that we simply multiply the points of the polygons by dividing each vertex into N small pieces.
        /// </summary>
        /// <param name="points"></param>
        /// <returns></returns>
        private static List<Vector2D> CorrectPolygonRepresentation(List<Vector2D> points)
        {
            if (points == null)
                throw new ArgumentNullException("points");

            int numberOfParts = 4;

            List<Vector2D> correctedPoints = new List<Vector2D>(points.Count * numberOfParts);

            Vector2D next = null;
            for (int i = 0; i < points.Count; ++i)
            {
                if (i < points.Count - 1)
                    next = points[i + 1];
                else
                    next = points[0];
                
                for (int j = 0; j < numberOfParts; ++j)
                {
                    correctedPoints.Add(points[i] + (next - points[i]) * (j / (float)numberOfParts));
                }
            }

            return correctedPoints;
        }

        #endregion

        #region Common conversions between Core and Library

        /// <summary>
        /// Converts a "core" vector to its equivavelent represenation in the library.
        /// </summary>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static Physics2DDotNet.Math2D.ALVector2D ConvertVectorCoreToLibrary(Vector2D vector)
        {
            return new Physics2DDotNet.Math2D.ALVector2D(0.0f, vector.X, vector.Y);
        }

        /// <summary>
        /// Convert a "library" vector to its core equivalent.
        /// </summary>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static Vector2D ConvertVectorLibraryToCore(Physics2DDotNet.Math2D.ALVector2D vector)
        {
            return new Vector2D(vector.Linear.X, vector.Linear.Y);
        }

        /// <summary>
        /// Convert a "library" vector to its core equivalent.
        /// </summary>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static Vector2D ConvertVectorLibraryToCore(AdvanceMath.Vector2D vector)
        {
            return new Vector2D(vector.X, vector.Y);
        }

        /// <summary>
        /// Converts an array of "core" vectors to an array of "library" vectors.
        /// </summary>
        /// <param name="list"></param>
        /// <returns></returns>
        public static AdvanceMath.Vector2D[] ConvertVectorArrayCoreToLibrary(List<Vector2D> list)
        {
            AdvanceMath.Vector2D[] vectors = new AdvanceMath.Vector2D[list.Count];
            for (int i = 0; i < list.Count; ++i)
                vectors[i] = new AdvanceMath.Vector2D(list[i].X, list[i].Y);

            return vectors;
        }

        /// <summary>
        /// Converts an array of "library" vectors to an array of "core" vectors.
        /// </summary>
        /// <param name="list"></param>
        /// <returns></returns>
        public static Vector2D[] ConvertVectorArrayLibraryToCore(AdvanceMath.Vector2D[] list)
        {
            Vector2D[] vectors = new Vector2D[list.Length];
            for (int i = 0; i < list.Length; ++i)
                vectors[i] = new Vector2D(list[i].X, list[i].Y);

            return vectors;
        }

        #endregion		
        
        #region Creating the library representation of the physical objects

        /// <summary>
		/// Creates a library representation of the "core" physical object.
		/// </summary>
		/// <param name="physicalObject"></param>
		/// <returns></returns>
		private Physics2DDotNet.Body CreateBody(IPhysicalObject simpleObject)
		{
			float mass = simpleObject.Mass;
			if (!simpleObject.IsMovable)
				mass = float.PositiveInfinity;
			
			Physics2DDotNet.Body entityBody = new Physics2DDotNet.Body(
				new Physics2DDotNet.PhysicsState(ConvertVectorCoreToLibrary(simpleObject.Position), ConvertVectorCoreToLibrary(simpleObject.Velocity)),
				new Physics2DDotNet.Circle(3.0f, 32),
				mass,
				new Physics2DDotNet.Coefficients(0.66f, 0.2f),
				new Physics2DDotNet.Lifespan());

			return entityBody;
		}

		/// <summary>
		/// Creates a library representation of the "core" polygonal object.
		/// </summary>
		/// <param name="polygonalObject"></param>
		/// <returns></returns>
		private Physics2DDotNet.Body CreateBody(Polygon polygonalObject)
		{
			float mass = polygonalObject.Mass;
			if (!polygonalObject.IsMovable)
				mass = float.PositiveInfinity;

			Physics2DDotNet.Body entityBody = new Physics2DDotNet.Body(
				new Physics2DDotNet.PhysicsState(ConvertVectorCoreToLibrary(polygonalObject.Position)),
				new Physics2DDotNet.Polygon(ConvertVectorArrayCoreToLibrary(CorrectPolygonRepresentation(polygonalObject.Points)), 0.75f),
				mass,
				new Physics2DDotNet.Coefficients(0.66f, 0.2f),
				new Physics2DDotNet.Lifespan());

			return entityBody;
        }

        #endregion

        #region Main update and synchronization logic

        /// <summary>
		/// Steps the engine with the given delta value.
		/// </summary>
		/// <param name="delta"></param>
		public void Step(float delta)
		{
            // Reset collisions storage
            collisions.Clear();

            // Iterate through physical objects and apply forces
            foreach (IPhysicalObject physicalObject in objects)
            {
                Physics2DDotNet.Body body = entitiesMap[physicalObject];

                ApplyForcesToBody(physicalObject.Forces, body);

                if (physicalObject is RobotPlayer)
                {
                    RobotPlayer robot = (RobotPlayer)physicalObject;
                    CorrectDriftingBehaviour(robot, body);
                    UpdateRobotBrakesAndDirection(robot, body);
                }
            }

			// Synchronize with the pysical engine (post-sync)
            for (float f = 0; f < delta; f += 0.001f)
                engine.Update(0.02f);

            time += delta;

            foreach (IPhysicalObject physicalObject in objects)
            {
                UpdateCorePhysicalObject(physicalObject, entitiesMap[physicalObject]);
            }
        }


        /// <summary>
 		/// Determines whether the robot is going forward or backward and apply the brakes.
        /// </summary>
        /// <param name="robot"></param>
        /// <param name="body"></param>
        private void UpdateRobotBrakesAndDirection(RobotPlayer robot, Physics2DDotNet.Body body)
        {
            // Apply brakes
            float brakeModifier = 0.2f;

            body.State.Velocity = body.State.Velocity *
                ((robot.MaxPower - brakeModifier * Math.Min(robot.Brake, robot.MaxPower)) / robot.MaxPower);

            robot.Brake = 0f;

            robot.MotorLeft = 0;
            robot.MotorRight = 0;

            // Set IsBackward property
            float velocityAngle = AdvanceMath.Vector2D.GetAngle(body.State.Velocity.Linear);
            float headingAngle = AdvanceMath.Vector2D.GetAngle(ConvertVectorCoreToLibrary(robot.Heading).Linear);

            // Correct intervals
            if (Math.Abs(velocityAngle - headingAngle) > Math.PI)
            {
                if (velocityAngle > headingAngle)
                {
                    velocityAngle -= 2 * (float)Math.PI;
                }
                else
                {
                    headingAngle -= 2 * (float)Math.PI;
                }
            }

            // We go backward if the angle betweeb the velocity and heading is more than 135 degrees
            if (Math.Abs(velocityAngle - headingAngle) > 3 * Math.PI / 4 && body.State.Velocity.Linear.Magnitude > 0.02f && !robot.IsBackward)
            {
                robot.IsBackward = true;
            }
            // We switch back to forward as soon as the angle gets below 45 degrees
            else if (Math.Abs(velocityAngle - headingAngle) < 1 * Math.PI / 4 && body.State.Velocity.Linear.Magnitude > 0.0f && robot.IsBackward)
            {
                robot.IsBackward = false;
            }

         }

        /// <summary>
        /// Updates the Core physical object according to the values calculated by the physical engine.
        /// </summary>
        /// <param name="physicalObject"></param>
        /// <param name="body"></param>
        private void UpdateCorePhysicalObject(IPhysicalObject physicalObject, Physics2DDotNet.Body body)
        {
            // Send the calculated positions back to the Core
            physicalObject.Position = ConvertVectorLibraryToCore(body.State.Position);

            // Get the rotation in degrees using the polygon's shape matrix
            Physics2DDotNet.Math2D.ALVector2D rotationVector =
                Physics2DDotNet.Math2D.ALVector2D.Transform(
                    body.Shape.Matrix, new Physics2DDotNet.Math2D.ALVector2D(0.0f, 1.0f, 0.0f));

            // It is in radians, lets convert it to degrees (sometimes it is easier to calculate with in the upper layers)
            physicalObject.Rotation = AdvanceMath.MathHelper.DegreesPerRadian * rotationVector.Angular;

            // And set the heading vector
            Vector2D originalHeadingVector = headingVectors[physicalObject];
            AdvanceMath.Vector2D rotatedHeadingVector =
                    AdvanceMath.Vector2D.Rotate(rotationVector.Angular, new AdvanceMath.Vector2D(originalHeadingVector.X, originalHeadingVector.Y));
            physicalObject.Heading = new Vector2D(rotatedHeadingVector.X, rotatedHeadingVector.Y);

            // As well as the core velocity
            physicalObject.Velocity = ConvertVectorLibraryToCore(body.State.Velocity);
        }

        /// <summary>
        /// If the object is a robot then correct its drifting behavior by adding an extra force. 
        /// Without this correction it's movement will look like skating on ice.
        /// </summary>
        /// <param name="physicalObject"></param>
        /// <param name="body"></param>
        private void CorrectDriftingBehaviour(RobotPlayer robotPlayer, Physics2DDotNet.Body body)
        {
            // Robot should rather go that way it is heading
            if (robotPlayer.Velocity.Length > 0.0)
            {
                float headingMultiplier = 1.0f;
                if (robotPlayer.IsBackward)
                {
                    headingMultiplier = -1.0f;
                }

                // Decreases the current velocity
                AdvanceMath.Vector2D brakeVect = body.State.Velocity.Linear.Normalized * -driftingCorrectionFactor * body.State.Velocity.Linear.Magnitude;

                // And applies the gained force in the direction of the heading vector
                AdvanceMath.Vector2D side = driftingCorrectionFactor * body.State.Velocity.Linear.Magnitude
                    * headingMultiplier * (ConvertVectorCoreToLibrary(robotPlayer.Heading.Normalized).Linear);

                body.ApplyForce(ref brakeVect);
                body.ApplyForce(ref side);
            }
        }

        /// <summary>
        /// Applies the forces that were given in the physicalObject.Forces collection to the library body 
        /// representing the object.
        /// </summary>
        /// <param name="vectors"></param>
        /// <param name="body"></param>
        private void ApplyForcesToBody(IEnumerable<FreeVector2D> vectors, Physics2DDotNet.Body body)
        {
            if (vectors != null)
            {
                // Apply forces given explicitly
                foreach (FreeVector2D vector in vectors)
                {
                    AdvanceMath.Vector2D start = ConvertVectorCoreToLibrary(vector.Start).Linear;
                    AdvanceMath.Vector2D end = ConvertVectorCoreToLibrary(vector.End).Linear;

                    if (vector.IsOrigoBased)
                    {
                        body.ApplyForce(ref end);
                    }
                    else
                    {
                        body.ApplyForce(ref end, ref start);
                    }
                }
            }
        }

        /// <summary>
        /// Updates the library body's position and heading with the core values.
        /// </summary>
        public void SynchronizeWithGame()
        {
            foreach (IPhysicalObject physicalObject in objects)
            {
                Physics2DDotNet.Body body = entitiesMap[physicalObject];
                body.State.Position = ConvertVectorCoreToLibrary(physicalObject.Position);
				body.State.Velocity = ConvertVectorCoreToLibrary(physicalObject.Velocity);
                RotateBody(physicalObject, body);
                UpdateCorePhysicalObject(physicalObject, body);
            }
        }

        /// <summary>
        /// Rotates the library body by the core heading change.
        /// </summary>
        /// <param name="physicalObject"></param>
        /// <param name="body"></param>
        private void RotateBody(IPhysicalObject physicalObject, Physics2DDotNet.Body body)
        {
            Physics2DDotNet.Math2D.Matrix2D rotMatrix = new Physics2DDotNet.Math2D.Matrix2D(
                    AdvanceMath.Matrix2x2.FromRotation(Vector2D.GetRotationAngle(headingVectors[physicalObject], physicalObject.Heading) / Vector2D.DegreesToRadian),
                    AdvanceMath.Matrix3x3.Identity);
            body.ApplyMatrix(ref rotMatrix);
        }

        /// <summary>
        /// Returns true if there was a collision in the previous step
        /// </summary>
        /// <returns></returns>
        public bool WasCollision()
        {
            if (collisions != null)
                return collisions.Count > 0;
            return false;
        }

        public void Clear()
        {
            bodiesMap.Clear();
            collisions.Clear();
            objects.Clear();
            headingVectors.Clear();
            entitiesMap.Clear();
            time = 0;
            engine.Clear();
            engine.AddLogicRange(logics);
        }

        /// <summary>
        /// Returns the objects that was collided with the physicalObject in the last moment.
        /// </summary>
        /// <param name="physicalObject"></param>
        /// <returns></returns>
        public IEnumerable<IPhysicalObject> GetLastColliders(IPhysicalObject physicalObject)
        {
            IEnumerable<CollisionInfo> collisionInfos =
                collisions.FindAll(delegate(CollisionInfo ci) { return ci.A == physicalObject || ci.B == physicalObject; });

            List<IPhysicalObject> physicalObjects = new List<IPhysicalObject>();

            foreach (CollisionInfo ci in collisionInfos)
            {
                if (ci.A == physicalObject)
                {
                    physicalObjects.Add(ci.B);
                }
                else
                {
                    physicalObjects.Add(ci.A);
                }
            }

            return physicalObjects;
        }

        #endregion

        #endregion

        #region Lifecycle

        /// <summary>
        /// Constructs the physical adapter.
        /// It will set the logic which slows down the objects simulating friction (KineticEnergyLimiter) and the engine's additional parameters.
        /// </summary>
		public Physics2DDotNetAdapter()
		{
			engine = new Physics2DDotNet.PhysicsEngine();
			objects = new List<IPhysicalObject>();
            entitiesMap = new PhysicalEntitiesToBodiesMap();
            bodiesMap = new BodiesToPhysicalEntitiesMap();
			headingVectors = new HeadingVectorsMap();
            collisions = new List<CollisionInfo>();

            ConfigureEngine();
        }

        private List<Physics2DDotNet.PhysicsLogic> logics = new List<Physics2DDotNet.PhysicsLogic>();

        /// <summary>
        /// Sets up the engine's the default values and reads configuration file.
        /// </summary>
        private void ConfigureEngine()
        {
            ReadConfigurationFile();

            KineticEnergyLimiter kineticEnergyLimiter = 
                new KineticEnergyLimiter(KineticEnergyLimiter.LimitationTypes.Linear, 
                this.frictionCoefficient, 
                this.ballFrictionCoefficient, 
                this.rotationCorrectionFactor,
                this);

            engine.AddLogic(kineticEnergyLimiter);

            logics.Add(kineticEnergyLimiter);

            engine.BroadPhase = (Physics2DDotNet.Detectors.BroadPhaseCollisionDetector)new Physics2DDotNet.Detectors.SweepAndPruneDetector();
            Physics2DDotNet.Solvers.SequentialImpulsesSolver solver = new Physics2DDotNet.Solvers.SequentialImpulsesSolver();
            solver.Iterations = 12;
            solver.SplitImpulse = true;
            solver.BiasFactor = 0.7f;
            solver.AllowedPenetration = 0.1f;
            engine.Solver = solver;
        }

        /// <summary>
        /// Reads a float value from the hashtable with the given key.
        /// </summary>
        /// <param name="collection"></param>
        /// <param name="propertyName"></param>
        /// <param name="value"></param>
        private void ReadValue(Hashtable collection, string propertyName, ref float value)
        {
            if (collection != null && collection.ContainsKey(propertyName))
            {
                string s = (string) collection[propertyName];

                if (!string.IsNullOrEmpty(s))
                {
                    float f = 0f;
                    if (float.TryParse(s, out f))
                    {
                        value = f;
                    }
                }
            }

        }

        /// <summary>
        /// Reads the sections from the application configuration file related to the physics engine.
        /// </summary>
        private void ReadConfigurationFile()
        {
            Hashtable collection = (Hashtable)System.Configuration.ConfigurationManager.GetSection("physicalEngineSettings");
            ReadValue(collection, "DriftingCorrectionFactor", ref this.driftingCorrectionFactor);
            ReadValue(collection, "RotationCorrectionFactor", ref this.rotationCorrectionFactor);
            ReadValue(collection, "FrictionCoefficient", ref this.frictionCoefficient);
            ReadValue(collection, "BallFrictionCoefficient", ref this.ballFrictionCoefficient);
        }

        #endregion

        #region ILookAheadPhysicsEngine Members

        private IPhysicsEngine Clone()
        {
            //Physics2DDotNetAdapter clonedAdapter = new Physics2DDotNetAdapter();
            //clonedAdapter.ballFrictionCoefficient = this.ballFrictionCoefficient;
            //clonedAdapter.bodiesMap = this.bodiesMap;
            return null;
        }

        /// <summary>
        /// Calculates the physics state of the world after the given amount of time.
        /// Not implemented yet.
        /// </summary>
        /// <param name="time"></param>
        /// <returns></returns>
        public World LookAhead(float time)
		{
            // Clone the current physics engine
            
			throw new Exception("The method or operation is not implemented.");
		}

		#endregion
	}
}
